
/*
 * 作者：勤为本
 * 勤为本的小店：https://shop333183734.taobao.com/
 */


#include <stdio.h>
#include <types.h>
#include <math.h>
#include "../lib/ls1c_i2c.h"
#include "../lib/ls1c_simulate_i2c.h"
#include "../lib/ls1c_pin.h"
#include "../lib/ls1c_delay.h"
#include "../lib/ls1c_irq.h"
#include "../lib/ls1c_gpio.h"
#include "DataScope_DP.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "motor.h"


/*
 * mpu6050 接 I2C2，采用引脚GPIO57和GPIO56的第四复用
 * SCL   -----------  CAMDATA7/GPIO57/I2C_SCL2
 * SDA   -----------  CAMDATA6/GPIO56/I2C_SDA2
 * INT   -----------  CAMDATA3/GPIO53
 */
#define MPU6050_I2C_SCL_PIN         (57)
#define MPU6050_I2C_SDA_PIN         (56)
#define MPU6050_INT_PIN				(53)


// MPU6050的I2C通信地址（地址的前7bit）
#define MPU6050_I2C_ADDR            (0x68)      // AD0=GND

// MPU6050的ID，寄存器who am i中的值
#define MPU6050_ID                  (MPU6050_I2C_ADDR)

// MPU6050寄存器地址
#define MPU6050_RA_SAMPLE_DIV           (25)    // 0x19
#define MPU6050_RA_CONFIG               (26)    // 0x1A
#define MPU6050_RA_GYRO_CONFIG          (27)    // 0x1B
#define MPU6050_RA_ACCEL_CONFIG         (28)    // 0x1C
#define MPU6050_RA_FIFO_EN              (35)    // 0x23
#define MPU6050_RA_INT_PIN_CFG          (55)    // 0x37
#define MPU6050_RA_INT_EN               (56)    // 0x38
#define MPU6050_RA_INT_STATUS           (58)    // 0x3A
#define MPU6050_RA_ACCEL_XOUT_H         (59)    // 0x3B
#define MPU6050_RA_GYRO_XOUT_H          (67)    // 0x43
#define MPU6050_RA_USER_CTRL            (106)   // 0x6A
#define MPU6050_RA_POWER_MANAGEMENT_1   (107)   // 0x6B
#define MPU6050_RA_POWER_MANAGEMENT_2   (108)   // 0x6C
#define MPU6050_RA_WHO_AM_I             (117)   // 0x75


// 时钟源
#define MPU6050_CLK_INTERNAL_8M_OSC     (0x00)
#define MPU6050_CLK_PLL_X_GYRO          (0x01)
#define MPU6050_CLK_PLL_Y_GYRO          (0x02)
#define MPU6050_CLK_PLL_Z_GYRO          (0x03)
#define MPU6050_CLK_EXTERNAL_32_768K    (0x04)

// 陀螺仪量程
#define MPU6050_GYRO_FSR_250            (0)
#define MPU6050_GYRO_FSR_500            (1)
#define MPU6050_GYRO_FSR_1000           (2)
#define MPU6050_GYRO_FSR_2000           (3)     // +-2000dps

// 陀螺仪LSB精度
#define MPU6050_GYRO_LSB_SENSITIVITY_32_8   (32.8)      // 32.8 LSB/dps   对应量程为 +-1000dps
#define MPU6050_GYRO_LSB_SENSITIVITY_16_4   (16.4)      // 16.4 LSB/dps   对应量程为 +-2000dps

// 加速度计量程
#define MPU6050_ACCEL_FSR_2             (0)     // +-2g
#define MPU6050_ACCEL_FSR_4             (1)
#define MPU6050_ACCEL_FSR_8             (2)
#define MPU6050_ACCEL_FSR_16            (3)

// 加速度计LSB精度（注意，必须是浮点）
#define ACCEL_LSB_SENSITIVITY_16384         (16384.0)    // 16384 LSB/g      对应量程 +-2g
#define ACCEL_LSB_SENSITIVITY_8192          (8192.0)     // 8192 LSB/g       对应量程 +-4g

// 采样率的取值范围
#define MPU6050_RATE_MAX                (1000)  // 1khz
#define MPU6050_RATE_MIN                (4)     // 4hz.
#define MPU6050_DEFAULT_HZ              (200)
#define MPU6050_HZ                      (50)	// 注意，此值只表示采样率和FIFO输出频率，不表示INT引脚的中断频率
												// 不论采样频率和FIFO输出频率是多少，INT引脚固定以200hz的频率输出中断脉冲

// 陀螺仪输出频率
#define MPU6050_OUTPUT_RATE             (1000)  // DLPF使能时的值


#define q30                             (1073741824.0f)
#define PI                              (3.14159265)
#define RADIAN_2_ANGLE(x)               (x*180/PI)  // 弧度转角度，反三角函数得到弧度后，用此宏将弧度转换为角度


// mpu6050的i2c信息
simulate_i2c_t mpu6050_i2c_info = { MPU6050_I2C_SCL_PIN, MPU6050_I2C_SDA_PIN, 1};

// 位置矩阵（即MPU6050的XYZ轴与欧拉角pitch,yaw,roll的对应关系）
static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};    // MPU6050水平放置，x轴朝前方

float angle_init = 0;			// 由于装配等产生的，mpu6050的坐标系与理论的坐标系之间的差值
								// 该值如何确定呢？将电路板水平放置，此时显示的倾角
float angle_balance = 0;		// 平衡倾角
float gyro_balance = 0;			// 平衡角速度
float gyro_turn = 0;			// 转向角速度
float accel_z = 0;				// z轴的加速度值


// PID系数
int balance_kp = 1513;		// 平衡的P系数
int balance_kd = 0;			// 平衡的d系数


// 电机的速度
extern volatile int motor_left_speed;
extern volatile int motor_right_speed;


/*
 * 通过i2c从mpu6050指定寄存器中的读出一个字节
 * @slave_addr i2c从机地址
 * @reg_addr 寄存器地址
 * @ret 读到的值
 */
inline u8 mpu6050_i2c_read_byte(u8 slave_addr, u8 reg_addr)
{
    u8 data = 0;
    
    // 写入寄存器地址
    simulate_i2c_start(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, (slave_addr << 1) | LS1C_I2C_DIRECTION_WRITE);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, reg_addr);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
//    simulate_i2c_stop(&mpu6050_i2c_info);

    // 读出寄存器的值
    simulate_i2c_start(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, (slave_addr << 1) | LS1C_I2C_DIRECTION_READ);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
    data = simulate_i2c_read_byte(&mpu6050_i2c_info);
    simulate_i2c_send_no_ack(&mpu6050_i2c_info);
    simulate_i2c_stop(&mpu6050_i2c_info);

    return data;
}


/*
 * 将一个字节数据通过i2c写入mpu6050指定寄存器
 * @slave_addr i2c从机地址
 * @reg_addr 寄存器地址
 * @data 待写入的数据
 */
inline void mpu6050_i2c_write_byte(u8 slave_addr, u8 reg_addr, u8 data)
{
    // 开始信号
    simulate_i2c_start(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, (slave_addr << 1) | LS1C_I2C_DIRECTION_WRITE);
    simulate_i2c_read_ack(&mpu6050_i2c_info);

    // 写入寄存器地址
    simulate_i2c_write_byte(&mpu6050_i2c_info, reg_addr);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
    
    // 写入数据
    simulate_i2c_write_byte(&mpu6050_i2c_info, data);
    simulate_i2c_read_ack(&mpu6050_i2c_info);

    // 结束信号
    simulate_i2c_stop(&mpu6050_i2c_info);

    return ;
}


/*
 * 通过i2c从mpu6050的指定的寄存器地址读出指定长度的数据
 * @slave_addr i2c从机地址
 * @reg_addr 寄存器地址
 * @len 需要读出的数据长度
 * @buf 存放数据的缓存
 */
inline int mpu6050_i2c_read_bytes(u8 slave_addr, u8 reg_addr, u8 len, u8 *buf)
{
    u8 i = 0;
    
    // 写入寄存器地址
    simulate_i2c_start(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, (slave_addr << 1) | LS1C_I2C_DIRECTION_WRITE);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, reg_addr);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
//    simulate_i2c_stop(&mpu6050_i2c_info);
    
    // 读出指定长度的数据
    simulate_i2c_start(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, (slave_addr << 1) | LS1C_I2C_DIRECTION_READ);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
    for (i = 0; len > 0; i++, len--)
    {
        buf[i] = simulate_i2c_read_byte(&mpu6050_i2c_info);
        if (1 == len)
        {
            simulate_i2c_send_no_ack(&mpu6050_i2c_info);
        }
        else
        {
            simulate_i2c_send_ack(&mpu6050_i2c_info);
        }
    }
    simulate_i2c_stop(&mpu6050_i2c_info);

    return 0;
}


/*
 * 通过i2c向指定地址写入指定长度的指定数据
 * @slave_addr i2c从机地址
 * @reg_addr 寄存器地址
 * @len 需要写入的长度
 * @buf 待写入的数据
 */
inline int mpu6050_i2c_write_bytes(u8 slave_addr, u8 reg_addr, u8 len, u8 *buf)
{
    u8 i = 0;
    
    simulate_i2c_start(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, (slave_addr << 1) | LS1C_I2C_DIRECTION_WRITE);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
    simulate_i2c_write_byte(&mpu6050_i2c_info, reg_addr);
    simulate_i2c_read_ack(&mpu6050_i2c_info);
    for (i = 0; i < len; i++)
    {
        simulate_i2c_write_byte(&mpu6050_i2c_info, buf[i]);
        simulate_i2c_read_ack(&mpu6050_i2c_info);
    }
    simulate_i2c_stop(&mpu6050_i2c_info);

    return 0;
}


/*
 * 设置数字低通滤波器带宽
 * @bandwidth 陀螺仪的带宽(hz)
 */
void mpu6050_set_DLPF(u16 bandwidth)
{
    u8 dlpf_cfg = 0;

    if ( bandwidth >= 188)
        dlpf_cfg = 1;
    else if ( bandwidth >= 98 )
        dlpf_cfg = 2;
    else if ( bandwidth >= 42 )
        dlpf_cfg = 3;
    else if ( bandwidth >= 20 )
        dlpf_cfg = 4;
    else if ( bandwidth >= 10 )
        dlpf_cfg = 5;
    else
        dlpf_cfg = 6;

    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_CONFIG, dlpf_cfg);

    return ;
}


/*
 * 设置采样率
 * @rate 采样率，取值范围[4,1000]hz
 */
void mpu6050_set_rate(u16 rate)
{
    u8 div = 0;
    
    // 设置sample rate divider
    if (MPU6050_RATE_MAX < rate) rate = MPU6050_RATE_MAX;       // 限制rate的最大值
    if (MPU6050_RATE_MIN > rate) rate = MPU6050_RATE_MIN;       // 限制rate的最小值
    div = MPU6050_OUTPUT_RATE / rate - 1;       // 寄存器手册中的公式  sample rate = gyroscope output rate / (1 + SMPLRT_DIV)
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_SAMPLE_DIV, div);

    // 设置数字低通滤波器DLPF频率
    mpu6050_set_DLPF( rate/2 );

    return ;
}


unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b = 0;

    if (0 < row[0])
        b = 0;
    else if (0 > row[0])
        b = 4;
    else if (0 < row[1])
        b = 1;
    else if (0 > row[1])
        b = 5;
    else if (0 < row[2])
        b = 2;
    else if (0 > row[2])
        b = 6;
    else
        b = 7;      // error;

    return b;
}


unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{
    unsigned short scalar = 0;

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;

    return scalar;
}


// 自检(+校准)
void mpu6050_self_test(void)
{
    int result = 0;
    long gyro[3] = {0};
    long accel[3] = {0};
    float gyro_sens = 0.0;
//    unsigned short accel_sens = 0;

    result = mpu_run_self_test(gyro, accel);
    if (0x7 == result)
    {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        // 校准陀螺仪
        mpu_get_gyro_sens(&gyro_sens);
        gyro[0] = (long)(gyro[0] + gyro_sens);
        gyro[1] = (long)(gyro[1] + gyro_sens);
        gyro[2] = (long)(gyro[2] + gyro_sens);
        dmp_set_gyro_bias(gyro);

        // 校准加速度计
        // 不校准加速度计，模块的初始方位不再受开机时模块位置的影响，避免开机时需要将小车扶正的麻烦
/*
        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
*/
    }
}


// 初始化dmp
int mpu6050_dmp_init(void)
{
    int ret = 0;
    
    if (! (ret = mpu_init()))    // 调用dmp库里面的初始化函数
    {
        // 设置需要的传感器（这里，陀螺仪和加速度计都需要）
        if (!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
            printf("[%s] mpu_set_sensors complete ......\r\n", __FUNCTION__);
        else 
            printf("[%s] mpu_set_sensors fail!\r\n", __FUNCTION__);

        // 设置fifo
        if (!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
            printf("[%s] mpu_configure_fifo complete ......\r\n", __FUNCTION__);
        else
            printf("[%s] mpu_configure_fifo fail!\r\n", __FUNCTION__);

        // 设置采样率
        if (!mpu_set_sample_rate(MPU6050_HZ))
            printf("[%s] mpu_set_sample_rate complete ......\r\n", __FUNCTION__);
        else
            printf("[%s] mpu_set_sample_rate fail!\r\n", __FUNCTION__);

        // 加载dmp固件
        if (!(ret = dmp_load_motion_driver_firmware()))
        {
            printf("[%s] dmp_load_motion_driver_firmware complete ......\r\n", __FUNCTION__);
        }
        else
        {
            printf("[%s] dmp_load_motion_driver_firmware fail! ret=%d\r\n", __FUNCTION__, ret);
            return ret;
        }

        // 设置陀螺仪方向
        if (!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
            printf("[%s] dmp_set_orientation complete ......\r\n", __FUNCTION__);
        else
            printf("[%s] dmp_set_orientation fail!\r\n", __FUNCTION__);

        // 设置dmp功能
        if (!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT          // 根据陀螺仪和加速度计融合计算四元数并输出，低功耗
                                | DMP_FEATURE_GYRO_CAL   		// 每次设备静止超过8秒时，校准陀螺仪零偏
                                | DMP_FEATURE_SEND_CAL_GYRO		// 将陀螺仪的原始数据放入FIFO中
                                | DMP_FEATURE_SEND_RAW_ACCEL))	// 将加速度计的原始数据放入FIFO中
            printf("[%s] dmp_enable_feature complete ......\r\n", __FUNCTION__);
        else
            printf("[%s] dmp_enable_feature fail!\r\n", __FUNCTION__);

        // 设置dmp输出频率
        if (!dmp_set_fifo_rate(MPU6050_HZ))
            printf("[%s] dmp_set_fifo_rate complete ...... \r\n", __FUNCTION__);
        else
            printf("[%s] dmp_set_fifo_rate fail!\r\n", __FUNCTION__);

        // 自检（+校准）
        // 因为get_st_biases（）中需要64位的除法，目前没有相关库函数，pmon中的do_div拷过来编译不过
        // 为了能够编译通过，将函数get_st_biases（）中相应代码注释掉了
//        mpu6050_self_test();  s

        // 使能
        if (!mpu_set_dmp_state(1))
            printf("[%s] mpu_set_dmp_state complete ......\r\n", __FUNCTION__);
        else
            printf("[%s] mpu_set_dmp_state fail!\r\n", __FUNCTION__);
    }

    return ret;
}


// 读取DMP值
void mpu6050_dmp_read(void)
{
    short gyro[3] = {0};
    short accel[3] = {0};
    long quat[4] = {0};
    unsigned long sensor_timestamp = 0;
    short sensors = 0;
    unsigned char more = 0;
    float q0 = 0.0;
    float q1 = 0.0;
    float q2 = 0.0;
    float q3 = 0.0;
    float pitch = 0.0;          // pitch为绕x轴的旋转角度
    int ret = 0;

    /*
        欧拉角是表达旋转的最简单的一种方式，形式上它是一个三维向量，其值分别代表物体绕坐标系三个轴(x,y,z轴）的旋转角度
        所以，pitch为绕x轴的旋转角度，
    */

    ret = dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
    if (ret)
    {
        printf("[%s] dmp_read_fifo fail. ret=%d\r\n", __FUNCTION__, ret);
        return ;
    }
    
    if (INV_WXYZ_QUAT & sensors)
    {
        q0 = quat[0] / q30;
        q1 = quat[1] / q30;
        q2 = quat[2] / q30;
        q3 = quat[3] / q30;
        pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
//        printf("[%s] pitch=% 8.3f\r\n", __FUNCTION__, pitch);
		angle_balance = pitch;		// 平衡倾角
    }
	gyro_balance = gyro[1] / MPU6050_GYRO_LSB_SENSITIVITY_16_4;	// 平衡角速度
	gyro_turn = gyro[2] / MPU6050_GYRO_LSB_SENSITIVITY_16_4;	// 转向角速度
	accel_z = accel[2] / ACCEL_LSB_SENSITIVITY_16384;			// z轴的加速度

    return ;
}


/*
 * 根据倾角和角速度，计算平衡时的pwm高电平宽度
 * @angle 倾角
 * @gyro 角速度
 */
int balance(float angle, float gyro)
{
	int pwm_high = 0;

	pwm_high = balance_kp * (angle - angle_init) + balance_kd * gyro;

	return pwm_high;
}


/*
 * mpu6050的中断处理函数
 * @IRQn 中断号
 * @param 传递给本中断处理函数的参数
 */
void mpu6050_int_irqhandler(int IRQn, void *param)
{
	static int count = 0;
	int pwm_high = 0;		// 高电平宽度（单位，系统时钟个数）
	
/*
 * INT引脚以200hz的频率输出脉冲，不论采样频率和FIFO输出频率是多少
 * 不是每次中断都能成功读出数据，需要根据FIFO输出频率来读FIFO，
 * 即不是每次中断都读FIFO
 * 当采样频率和FIFO输出频率 小于 200hz（默认频率）时，需要做下处理
 */

	// FIFO频率输出频率为100hz时，间隔一次中断，读一次FIFO；50hz时，间隔三次，以此类推
	count++;
	if (3 > count)
	{
		return ;
	}
	count = 0;

	// 读取dmp值
	mpu6050_dmp_read();

	// 读取电机编码器的值
	motor_encoder_get_speed();

	// 平衡
	pwm_high = balance(angle_balance, gyro_balance);

	// 控制电机
	motor_control(angle_balance - angle_init, pwm_high, pwm_high);

	// 发送到上位机显示（波形），主要用于调试
    DataScope_Show_dmp_data(angle_balance, 
    						gyro_balance, 
    						gyro_turn, 
    						accel_z, 
    						motor_left_speed, 
    						motor_right_speed);

	return ;
}


// mpu6050的中断初始化
void mpu6050_interrupt_init(void)
{
	unsigned int int_gpio = MPU6050_INT_PIN;	// mpu6050的INT引脚
	int int_irq = LS1C_GPIO_TO_IRQ(int_gpio);	// 中断号

	gpio_init(int_gpio, gpio_mode_input);
	gpio_set_irq_type(int_gpio, IRQ_TYPE_EDGE_FALLING);	// 下降沿触发中断
	irq_install(int_irq, mpu6050_int_irqhandler, NULL);
	irq_enable(int_irq);
}


/*
 * MPU6050初始化
 * @ret 成功 or 失败
 */
int mpu6050_init(void)
{
    int ret = 0;
    u8 id = 0;
    
    simulate_i2c_init(&mpu6050_i2c_info);

    // 延时一会（裸机程序启动速度太快）
    delay_ms(200);

    // 读出MPU6050的ID，以此判断I2C通信是否正常
    id = mpu6050_i2c_read_byte(MPU6050_I2C_ADDR, MPU6050_RA_WHO_AM_I);
    if (MPU6050_ID != id)
    {
        printf("[%s] mpu6050 init fail. device id=0x%x\r\n", __FUNCTION__, id);
        return -1;
    }
    printf("[%s] mpu6050 id=0x%x\r\n", __FUNCTION__, id);

    //复位并唤醒MPU6050
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_POWER_MANAGEMENT_1, 0x80);    // 复位MPU6050
    delay_ms(100);
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_POWER_MANAGEMENT_1, 0x00);     // 唤醒mpu6050
    delay_ms(100);

    // 配置MPU6050
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FSR_2000 << 3);     // 陀螺仪量程为+-2000dps
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FSR_2 << 3);      // 加速度计量程为+-2g
    mpu6050_set_rate(MPU6050_HZ);   // 设置采样率
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_INT_EN, 0x00);        // 关闭所有中断
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_USER_CTRL, 0x00);     // I2C主模式关闭，不让MPU6050控制AUXI2C
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_FIFO_EN, 0x78);       // 打开陀螺仪和加速度计的FIFO
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_INT_PIN_CFG, 0x80);   // INT引脚低电平有效，龙芯1C的I2C与MPU6050的AUXI2C直通，可以直接访问AUXI2C上的芯片（如果有的话）
    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_POWER_MANAGEMENT_1, MPU6050_CLK_PLL_Y_GYRO);  // 设置陀螺仪的Y轴作为参考时钟
//    mpu6050_i2c_write_byte(MPU6050_I2C_ADDR, MPU6050_RA_POWER_MANAGEMENT_2, 0x00);    // 加速度和陀螺仪都工作

    // 经过前面的初始化，已经可以读出陀螺仪和加速度计的原始数据了，如果不需要使用dmp获取欧拉角和四元数的话，可以不使用dmp
    // 初始化dmp
    if((ret = mpu6050_dmp_init()))
    {
        printf("[%s] dmp init fail. ret=%d\r\n", __FUNCTION__, ret);
        return ret;
    }

	// 中断初始化
	mpu6050_interrupt_init();

    return 0;
}



/*
 * 获取陀螺仪的AD值
 * @gx_adc, @ggy_adcy, @gz_adc 陀螺仪x、y、z轴的AD值（带符号）
 */
void mpu6050_get_gyroscope_adc(s16 *gx_adc, s16 *gy_adc, s16 *gz_adc)
{
    u8 buf[6] = {0};

    mpu6050_i2c_read_bytes(MPU6050_I2C_ADDR, MPU6050_RA_GYRO_XOUT_H, 6, buf);
    *gx_adc = ((u16)buf[0] << 8) | buf[1];
    *gy_adc = ((u16)buf[2] << 8) | buf[3];
    *gz_adc = ((u16)buf[4] << 8) | buf[5];

    return ;
}


/*
 * 获取加速度计的原始值
 * @ax, @ay, @az 加速度计的原始数据(带符号)
 */
void mpu6050_get_accelerometer(s16 *ax, s16 *ay, s16 *az)
{
    u8 buf[6] = {0};

    mpu6050_i2c_read_bytes(MPU6050_I2C_ADDR, MPU6050_RA_ACCEL_XOUT_H, 6, buf);
    *ax = ((u16)buf[0] << 8) | buf[1];
    *ay = ((u16)buf[2] << 8) | buf[3];
    *az = ((u16)buf[4] << 8) | buf[5];

    return ;
}


// 获取原始数据
void mpu6050_original_data(void)
{
    s16 gx_adc=0, gy_adc=0, gz_adc=0;
    s16 ax_adc=0, ay_adc=0, az_adc=0;
    float gx = 0.0, gy = 0.0, gz = 0.0;
    float ax = 0.0, ay = 0.0, az = 0.0;

    // 获取陀螺仪的AD值
    mpu6050_get_gyroscope_adc(&gx_adc, &gy_adc, &gz_adc);

    // 根据AD值计算原始数据
    gx = gx_adc / MPU6050_GYRO_LSB_SENSITIVITY_16_4;
    gy = gy_adc / MPU6050_GYRO_LSB_SENSITIVITY_16_4;
    gz = gz_adc / MPU6050_GYRO_LSB_SENSITIVITY_16_4;

    // 获取加速度计的AD值
    mpu6050_get_accelerometer(&ax_adc, &ay_adc, &az_adc);

    // 根据AD值计算原始数据
    ax = ax_adc / ACCEL_LSB_SENSITIVITY_16384;
    ay = ay_adc / ACCEL_LSB_SENSITIVITY_16384;
    az = az_adc / ACCEL_LSB_SENSITIVITY_16384;

//    printf("[%s] gx_adc=%6d, gy_adc=%6d, gz_adc=%6d,    ax_adc=%6d, ay_adc=%6d, az_adc=%6d\r\n",
//            __FUNCTION__, gx_adc, gy_adc, gz_adc,   ax_adc, ay_adc, az_adc);
//    printf("[%s] gx=% 8.3f, gy=% 8.3f, gz=% 8.3f,   ax=% 8.3f, ay=% 8.3f, az=% 8.3f\r\n",
//            __FUNCTION__,   gx, gy, gz,   ax, ay, az);

    // 发送到上位机
    DataScope_Show_original_data(gx, gy, gz, ax, ay, az);

    return ;
}


// 测试mpu6050是否正常工作
void mpu6050_test(void)
{
    // 读取DMP值
//    mpu6050_dmp_read();
	printf("[%s] ......\r\n", __FUNCTION__);
	
    return ;
}


